#include "vit_hwt901.hpp"

#ifndef VIT_HWT901_USE_CONFIG_H
#define VIT_HWT901_NAME  ("vit_hwt901")

#define VIT_HWT901_RS485_PATH  ("/dev/ttyS7")
#define VIT_HWT901_RS485_BAUDRATE  (230400)
#define VIT_HWT901_RS485_DE_PATH   ("/dev/gpiochip3")
#define VIT_HWT901_RS485_DE_PIN  (24)
#define VIT_HWT901_RS485_FRAME_TIMEOUT_MS  (100)

#define VIT_HWT901_MODBUS_SLAVE_ADDR (0x51)
#endif // !VIT_HWT901_USE_CONFIG_H



uart_v2 *Pvit_hwt901_uart = NULL;
rs485_v3 *Pvit_hwt901_rs485 = NULL;
agile_modbus_rtu_master_simple *Pvit_hwt901_rtu_master = NULL;

static yj_return init_flag = YJ_ERROR;

int vit_hwt901_rs485_modbus_read(uint8_t *data)
{
    if (!Pvit_hwt901_rs485 || !Pvit_hwt901_rtu_master || !Pvit_hwt901_uart || !data)
        return -1;
    return Pvit_hwt901_rs485->rs485_recv(data, MAX_ADU_LENGTH);
}

int vit_hwt901_rs485_modbus_write(uint8_t *data, uint32_t len)
{
    if (!Pvit_hwt901_rs485 || !Pvit_hwt901_rtu_master || !Pvit_hwt901_uart || !data)
        return -1;
    return Pvit_hwt901_rs485->rs485_send(data, len);
}

yj_return vit_hwt901_init(void)
{
    std::string name(VIT_HWT901_NAME);
    Pvit_hwt901_uart = new uart_v2(VIT_HWT901_RS485_PATH, VIT_HWT901_RS485_BAUDRATE, 8, 1);
    Pvit_hwt901_rs485 = new rs485_v3(Pvit_hwt901_uart,VIT_HWT901_RS485_FRAME_TIMEOUT_MS);
    // Pvit_hwt901_rs485 = new rs485_v3(Pvit_hwt901_uart, VIT_HWT901_RS485_DE_PATH, VIT_HWT901_RS485_DE_PIN, VIT_HWT901_RS485_FRAME_TIMEOUT_MS);
    Pvit_hwt901_rtu_master = new agile_modbus_rtu_master_simple(name,
                                                       vit_hwt901_rs485_modbus_write,
                                                       vit_hwt901_rs485_modbus_read);

    if (Pvit_hwt901_rs485->rs485_open() != YJ_OK)
        return init_flag;
    if (Pvit_hwt901_rtu_master->init() != 0)
        return init_flag;
    init_flag = YJ_OK;
    return init_flag;
}

void vit_hwt901_deinit(void)
{
    if (Pvit_hwt901_rs485)
    {
        Pvit_hwt901_rs485->rs485_close();
        delete Pvit_hwt901_rs485;
        Pvit_hwt901_rs485 = NULL;
    }
    if (Pvit_hwt901_rtu_master)
    {
        delete Pvit_hwt901_rtu_master;
        Pvit_hwt901_rtu_master = NULL;
    }
    if (Pvit_hwt901_uart)
    {
        delete Pvit_hwt901_uart;
        Pvit_hwt901_uart = NULL;
    }
}

yj_return vit_hwt901_read_all(vit_hwt901_data_t *data)
{
    std::vector<uint16_t> dataTorecv(21);
    float accel_quick_cal = 16*9.8/32768;// m/s^2
    float ang_vel_quick_cal = 2000/32768;// °/s
    float ang_quick_cal = 180/32768;// °


    if (init_flag == YJ_ERROR || !data)
    {
        LOG(ERROR) << "❌ vit_hwt901_read_all init_flag error or data is null";

        return YJ_ERROR;
    }
        
        if(Pvit_hwt901_rtu_master->read_holding_register_multiple(VIT_HWT901_MODBUS_SLAVE_ADDR,52,dataTorecv) < 0)
        {
            LOG(ERROR) << "❌ vit_hwt901_read_all read holding regs error";
            return YJ_ERROR;
        }

        data->X_accel = (short)dataTorecv[0]*16*9.8/32768;
        data->Y_accel = (short)dataTorecv[1]*16*9.8/32768;
        data->Z_accel = (short)dataTorecv[2]*16*9.8/32768;
        data->X_ang_vel = (short)dataTorecv[3]*2000/32768;
        data->Y_ang_vel = (short)dataTorecv[4]*2000/32768;
        data->Z_ang_vel = (short)dataTorecv[5]*2000/32768;
        data->X_angle = (short)dataTorecv[9]*180/32768;
        data->Y_angle = (short)dataTorecv[10]*180/32768;
        data->Z_angle = (short)dataTorecv[11]*180/32768;
       
        data->temperature = (short)dataTorecv[12]/100.0;    //°c
        data->air_pressure_Height = (((int)dataTorecv[20])<<16 | dataTorecv[19] )/100.0;// m

    return YJ_OK;

}

yj_return vit_hwt901_read_test(uint16_t *data)
{
    std::vector<uint16_t> dataTorecv(1);


    if (init_flag == YJ_ERROR || !data)
    {
        LOG(ERROR) << "❌ vit_hwt901_read_all init_flag error or data is null";

        return YJ_ERROR;
    }
        
        if(Pvit_hwt901_rtu_master->read_holding_register_multiple(VIT_HWT901_MODBUS_SLAVE_ADDR,0,dataTorecv) < 0)
        {
            LOG(ERROR) << "❌ vit_hwt901_read_all read holding regs error";
            return YJ_ERROR;
        }
     *data = dataTorecv[0]; 

    return YJ_OK;

}

void vit_hwt901_print_data(vit_hwt901_data_t *data)
{
    if (!data)
    {
       LOG(ERROR) << "vit_hwt901_print_data data is null";
       return;
    }
    
    LOG(INFO) << "\n============= data start =============\n" 
    << "X_accel: " << data->X_accel << "m/s^2 " << "Y_accel: "<< data->Y_accel << "m/s^2 " << "Z_accel: " << data->Z_accel << "m/s^2\n"
    << "X_ang_vel: " << data->X_ang_vel << "°/s " << "Y_ang_vel: "<< data->Y_ang_vel << "°/s " << "Z_ang_vel: " << data->Z_ang_vel << "°/s\n"
    << "X_angle: " << data->X_angle << "° " << "Y_angle: "<< data->Y_angle << "° " << "Z_angle: " << data->Z_angle << "°\n"
    << "temperature: " << data->temperature << "°C " << "air_pressure_Height: " << data->air_pressure_Height << "m\n"
    << "============= data end =============\n";
}